//serial_demo.cpp
#include <ros/ros.h>
#include<unistd.h>//提供通用的文件、目录、程序及进程操作的函数
#include <serial/serial.h>
#include <iostream>
#include <vector>

#include "std_msgs/String.h"

#include <sensor_msgs/JointState.h>
#include <moveit_msgs/DisplayTrajectory.h>

//自定义消息
#include "serial_demo/ti5_targetPos.h"
#include "serial_demo/ti5_polyJC_Lists.h"
#include "serial_demo/ti5_motorStatus.h"


#include<sys/types.h>//数据类型定义
#include<netinet/in.h>//定义数据结构sockaddr_in
#include<arpa/inet.h>//提供IP地址转换函数
#include<sys/socket.h>//提供socket函数及数据结构



#include "nlohmann/json.hpp"
using json = nlohmann::json;


#define err(errMsg) do{perror(errMsg);exit(EXIT_FAILURE);}while(0)//输出错误信息

#define MESSAGE_FREQ 100
#define MESSAGE_FREQ_END 60
using namespace std;

double next_velocitie[6]={0};

double ori_velocitie[6]={0} ;
double cha_velocitie[6]={0} ;
double now_velocitie[6]={0} ;


char buffer[5120];


//关节角度零位误差
//  3  5 inv
int t1=800; //85000;//  t1=45000;    t1=-90000;
int t2=33100;//50000;
int t3=3700;//25000;
int t4=55200;//35000;
int t5=18400;//40000;
int t6=0;//5000
int t7 = 0;//-400000;//注: 发送关节角时未加上t7


double degTrans = 18386.4888889;//  (101*65536) /(360)
double degTrans_poly = 18386.4888889;// (101*65536) /(360)
double degTransDiv1k = 1053.4682133;//  (101*65536/1000) /(2*pi)

//json decodeData
struct jsonData
{
    /* data */
    std::vector<double> joint1;
    std::vector<double> joint2;
    std::vector<double> joint3;
    std::vector<double> joint4;
    std::vector<double> joint5;
    std::vector<double> joint6;
    std::vector<double> gripper;
    std::vector<double> time;

};

string jsonKeyStr[] = {"joint1","joint2","joint3","joint4","joint5","joint6","gripper","time"};
std::vector<string> jsonKey(jsonKeyStr,jsonKeyStr+8);



class Listener
 {
    private:
        char topic_message[2048] = { 0 };

    public:
        int sCfd;//客户端套接字，只有一个
        int sSfd;//要连接的服务器套接字，只有一个
        struct sockaddr_in addr;//要连接服务器的地址
        ros::Publisher motorPub;
        ros::NodeHandle nh;
        //void callback(const sensor_msgs::JointState::ConstPtr &msg);
        // void callback(const moveit_msgs::DisplayTrajectory::ConstPtr &msg);
        void callback(const serial_demo::ti5_polyJC_Lists::ConstPtr &msg);
        void socketCcallback(const jsonData jData);
        char* getMessageValue();
        int createSocketClient();
        int32_t readCommand( double* poseList);
        Listener();
        ~Listener();
};


Listener::Listener()
{
    //设置需要连接的服务器的地址
    addr.sin_family = AF_INET;
    addr.sin_port = htons(38899);
    addr.sin_addr.s_addr = inet_addr("10.206.8.36");//112.64.195.47  10.206.114.53 
    /*=     inet_addr("127.0.0.1");
        =     inet_aton("127.0.0.1",&addr.sin_addr);*/
    
    this->motorPub = nh.advertise<serial_demo::ti5_motorStatus>("ti5_motor_status",1);


}
Listener::~Listener()
{
    close(this->sCfd);
}

/*创建TCP客户端并连接服务器，失败返回-1*/
int Listener::createSocketClient()
{
        //1.创建一个套接字用于通信
	/*socket（）
        pram1:af 地址协议簇  ipv4  ipv6
        pram2:type 传输协议类型  流式套接字 数据报  
        pram3:protocl 使用具体的某个传输协议
        成功返回非负整数，它与文件描述符类似，可称为套接口描述字，简称套接字；
        失败返回-1。
	*/
    this->sCfd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
    if(-1 == this->sCfd)
    {
        err("socketServer");
        return -1;
    }

    //2.与服务器建立连接
    /*connect（）
        pram1:socket()函数返回的套接字
        pram2:要绑定的地址
        pram3:地址长度
        成功则套接字(pram1)变为i已连接状态；失败返回-1。
    */
   this->sSfd = connect(this->sCfd, (struct sockaddr*) &this->addr, sizeof(this->addr));
    if(-1 == this->sSfd)
    {
        err("connect");
        return -1;
    }
    

    return 0;
}

//将s里的所有delim去除，并将s中被delim分割的所有子串集合成字符串数组elems
std::vector<std::string> vStringSplit(const  std::string& s, const std::string& delim="\n")
{
    std::vector<std::string> elems;
    size_t pos = 0;
    size_t len = s.length();
    size_t delim_len = delim.length();
    if (delim_len == 0) return elems;
    while (pos < len)
    {
        int find_pos = s.find(delim, pos);
        if (find_pos < 0)
        {
            elems.push_back(s.substr(pos, len - pos));
            break;
        }
        elems.push_back(s.substr(pos, find_pos - pos));
        pos = find_pos + delim_len;
    }
    return elems;
}


int32_t crc32_table[256]={0,1996959894,-301047508,-1727442502,124634137,1886057615,-379345611,-1637575261,	249268274,2044508324,-522852066,-1747789432,162941995,2125561021,-407360249,-1866523247,498536548,1789927666,-205950648,-2067906082,450548861,1843258603,-187386543,-2083289657,325883990,1684777152,-43845254,-1973040660,	335633487,1661365465,-99664541,-1928851979,997073096,1281953886,-715111964,-1570279054,1006888145,1258607687,-770865667,-1526024853,901097722,1119000684,-608450090,-1396901568,853044451,1172266101,-589951537,-1412350631,	651767980,1373503546,-925412992,-1076862698,565507253,1454621731,-809855591,-1195530993,671266974,1594198024,-972236366,-1324619484,795835527,1483230225,-1050600021,-1234817731,1994146192,31158534,-1731059524,-271249366,	1907459465,112637215,-1614814043,-390540237,2013776290,251722036,-1777751922,-519137256,2137656763,141376813,-1855689577,-429695999,1802195444,476864866,-2056965928,-228458418,1812370925,453092731,-2113342271,-183516073,	1706088902,314042704,-1950435094,-54949764,1658658271,366619977,-1932296973,-69972891,1303535960,984961486,-1547960204,-725929758,1256170817,1037604311,-1529756563,-740887301,1131014506,879679996,-1385723834,-631195440,	1141124467,855842277,-1442165665,-586318647,1342533948,654459306,-1106571248,-921952122,1466479909,544179635,-1184443383,-832445281,1591671054,702138776,-1328506846,-942167884,1504918807,783551873,-1212326853,-1061524307,-306674912,-1698712650,62317068,1957810842,-355121351,-1647151185,81470997,1943803523,-480048366,-1805370492,225274430,2053790376,-468791541,-1828061283,167816743,2097651377,-267414716,-2029476910,503444072,1762050814,-144550051,-2140837941,426522225,1852507879,-19653770,-1982649376,282753626,1742555852,-105259153,-1900089351,397917763,1622183637,-690576408,-1580100738,953729732,1340076626,-776247311,-1497606297,1068828381,1219638859,-670225446,-1358292148,906185462,1090812512,-547295293,-1469587627,829329135,1181335161,-882789492,-1134132454,628085408,1382605366,-871598187,-1156888829,570562233,1426400815,-977650754,-1296233688,733239954,1555261956,-1026031705,-1244606671,752459403,1541320221,-1687895376,-328994266,1969922972,40735498,-1677130071,-351390145,1913087877,83908371,-1782625662,-491226604,2075208622,213261112,-1831694693,-438977011,2094854071,198958881,-2032938284,-237706686,1759359992,534414190,-2118248755,-155638181,1873836001,414664567,-2012718362,-15766928,1711684554,285281116,-1889165569,-127750551,1634467795,376229701,-1609899400,-686959890,1308918612,956543938,-1486412191,-799009033,1231636301,1047427035,-1362007478,-640263460,1088359270,936918000,-1447252397,-558129467,1202900863,817233897,-1111625188,-893730166,1404277552,615818150,-1160759803,-841546093,1423857449,601450431,-1285129682,-1000256840,1567103746,711928724,-1274298825,-1022587231,1510334235,755167117};

int16_t table_s[]={12336,12592,12848,13104,13360,13616,13872,14128,14384,14640,16688,16944,17200,17456,17712,17968,12337,12593,12849,13105,13361,13617,13873,14129,14385,14641,16689,16945,17201,17457,17713,17969,12338,12594,12850,13106,13362,13618,13874,14130,14386,14642,16690,16946,17202,17458,17714,17970,12339,12595,12851,13107,13363,13619,13875,14131,14387,14643,16691,16947,17203,17459,17715,17971,12340,12596,12852,13108,13364,13620,13876,14132,14388,14644,16692,16948,17204,17460,17716,17972,12341,12597,12853,13109,13365,13621,13877,14133,14389,14645,16693,16949,17205,17461,17717,17973,12342,12598,12854,13110,13366,13622,13878,14134,14390,14646,16694,16950,17206,17462,17718,17974,12343,12599,12855,13111,13367,13623,13879,14135,14391,14647,16695,16951,17207,17463,17719,17975,12344,12600,12856,13112,13368,13624,13880,14136,14392,14648,16696,16952,17208,17464,17720,17976,12345,12601,12857,13113,13369,13625,13881,14137,14393,14649,16697,16953,17209,17465,17721,17977,12353,12609,12865,13121,13377,13633,13889,14145,14401,14657,16705,16961,17217,17473,17729,17985,12354,12610,12866,13122,13378,13634,13890,14146,14402,14658,16706,16962,17218,17474,17730,17986,12355,12611,12867,13123,13379,13635,13891,14147,14403,14659,16707,16963,17219,17475,17731,17987,12356,12612,12868,13124,13380,13636,13892,14148,14404,14660,16708,16964,17220,17476,17732,17988,12357,12613,12869,13125,13381,13637,13893,14149,14405,14661,16709,16965,17221,17477,17733,17989,12358,12614,12870,13126,13382,13638,13894,14150,14406,14662,16710,16966,17222,17478,17734,17990};

char* Listener::getMessageValue() 
{
    return topic_message;
}

//获取电流速度位置，从把字符数组l的 下标a到b-1的内容复制给v
void getdata(char* l,char* v,int a,int b)
{
        int j=0;
        for(int i=a;i<b;i++){
                v[j]=l[i];
                j++;
        }
//	std::cout<<v<<std::endl;
}
//将u的后l个字符 低位在前转为高位在前 保存到inv
void ivs(char* u,string& inv,int l)
{
        int j=0;
        for(int i=l-1;i>0;i-=2){
            inv[j++]=u[i-1];
                inv[j++]=u[i];
        }
}

//字符转16进制
int translate(string& inv,int l)
{
        unsigned int value=0;
        char OX2H[16]={'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'};
        unsigned int j;
        for(int i=0;i<l;i++){
                for(j=0;j<16;j++)
                        if(OX2H[j]==inv[i])
                                value=value*16+j;
        }
        return (int)value;
}

//无符号转有符号
int u2int(unsigned int u)
{
        unsigned int a=32708;
        int v;
	if(u>=a){
                v=u%a;
		v=v-a;
	}
        else
            v=u;
        return v;


}


//创建一个全局serial对象 

serial::Serial sp;

//将dest数组转为要发送到串口的数组，返回发送字节大小
size_t MemToStrStream(char* dest, const char* sourse, uint32_t size,char head,char end)
{
    uint32_t i = 0;
    uint8_t *pindex = (uint8_t *)sourse;
    *dest = head;
    int16_t *p = (int16_t *)(dest+1);
    for(i=0;i<size;i++)
    {
        p[i] = table_s[pindex[i]];
    }
    p[i] = end;
    //    qDebug()<<"end:"<<dest[size*2+1]<<dest[size*2+2];
    //    printf("**%d**%d**,",dest[size*2+1],dest[size*2+2]);
    return size*2+2;
}


uint32_t calcCRC32(uint32_t crc, uint8_t *string, uint32_t size)
{
    while(size--)
        crc = (crc >> 8)^(crc32_table[(crc ^ *string++)&0xff]);
    return crc;
}


void bufferTransfor(const std::string& buffer )
{
        std::string::size_type position;
        //position=buffer.find("\n");

        int index = buffer.find("\n");
                uint8_t mem[400]={0},memIndex=0;
        if (index>5 )
        {//1

                std::string frame = buffer.substr(1,index-1);
                while (true)
                {
                        if(frame.length()>1)
                        {
                std::string w1 = frame.substr(0,2);
                frame.erase(0,2);
                        }
                        else{break;}

                }

        }//1


}


int32_t s2t(std::string arr,int n)//char change to 16#digit
{
     int temp;
     //char* arr = new char[9];
     long long sum = 0;             //用long long防止结果过大
     for (int i = 0; i<n; i++)         //最后一位是'\0'，不用算进去
     {
         switch (arr[i])
         {
         case 'A': temp = 10; break;
         case 'B': temp = 11; break;
         case 'C': temp = 12; break;
         case 'D': temp = 13; break;
         case 'E': temp = 14; break;
         case 'F': temp = 15; break;
         default: temp = arr[i] - '0'; break;
         }
         sum = sum + temp * pow(16, n - 1- i);
     }
     return sum;
}


int32_t sixteen2ten(std::string tmpChar)//16#digit change to 10#digit
{
        std::string n1 = tmpChar.substr(0,2);
        std::string n2 = tmpChar.substr(2,2);
        std::string n3 = tmpChar.substr(4,2);
	//int a3 = s2t(n3);
	n3+=n2;
	n3+=n1;
	int endRes = s2t(n3,6);
	return endRes;
}


/*电机角度(int型  弧度*1000)   转换为需要发送的指令(uint32_t)
    param1:需要转换的参数个数
    param2:需要要转换的弧度地址  地址内参数示例：1570表示 1.57rad )
    param3:转换后的参数地址
*/
int radTransfor(int8_t num, int* rad, uint32_t* param)
{
    
    for(int i=0; i<num; i++)
    {
        *(param++)=(int)((*(rad++))*(degTrans/1000));
    }
  
    return 0;
}


//get data from json to struct jsonData
jsonData Json2Struct(const json j)
{
   
    //get data to struct jData
    jsonData jData;
    
    j.at("joint1").get_to(jData.joint1);
    j.at("joint2").get_to(jData.joint2);
    j.at("joint3").get_to(jData.joint3);
    j.at("joint4").get_to(jData.joint4);
    j.at("joint5").get_to(jData.joint5);
    j.at("joint6").get_to(jData.joint6);
    j.at("gripper").get_to(jData.gripper);
    j.at("time").get_to(jData.time);


    // int i=0;
    // for(auto it =jData.joint1.begin();it!=jData.joint1.end(); ++it)
    // {
        
    //     printf("joint1[%d]:%.4f",i,*it);
    //     i++;
    // }
    // i=0;
    // for(auto it =jData.joint2.begin();it!=jData.joint2.end(); ++it)
    // {
        
    //     printf("joint2[%d]:%.4f",i,*it);
    //     i++;
    // }


    return jData;
}


int32_t sendCommand(uint8_t numOfActuator, uint8_t *canIdList, uint8_t *commandList, uint32_t *parameterList)
{
    
    uint8_t buf[400],index=0;
    buf[index++] = numOfActuator;
    if(numOfActuator==0)
    {
        buf[index++] = canIdList[0];
        buf[index++] = commandList[0];
        buf[index] = parameterList[0];
        index +=4;
    }
    else
    {
        for(int i=0;i<numOfActuator;i++)
        {
            buf[index++] = canIdList[i];
        }
        for(int i=0;i<numOfActuator;i++)
        {
            buf[index++] = commandList[i];
        }
        for(int i=0;i<numOfActuator;i++)
        {
            *(uint32_t *)(buf+index) = parameterList[i];//4个字节的参数
            index+=4;
        }
    }

    *(uint32_t *)(buf+index) = calcCRC32(0,buf,index);
    index+=4;
    char uartTxBuf[2000];
    uint32_t byteToWrite = MemToStrStream(uartTxBuf,(char*)buf,index,'<','\n');
    size_t writeSize=0;
    

    //std::cout<<sp.write(uartTxBuf) <<std::endl;

    int numOfuartTxBuf  = sp.write(uartTxBuf);//向串口发送数据

    // std::cout<<"***numOfActuator:"<<(int)numOfActuator<<"***numOfuartTxBuf:"<<numOfuartTxBuf<<std::endl;
    // std::cout<<"***uartTxBuf***:"<<uartTxBuf<<std::endl;
    return 0;
}//end sendCommand


//get joints' I V S from serial port string and send joints' I V S to matlab by socketClient
int32_t Listener::readCommand(double* poseList)//the new function  readCommand,
{
    std::cout<<"***************into readCommand()***************"<<std::endl;
    //读取电机位置
    uint8_t canidlist1[10]={0},cmd1[10]={8};
    uint32_t parameterlist1[10]={0};
    std::cout<<"readcmd called sendcmd"<<std::endl;
    sendCommand(0,canidlist1,cmd1,parameterlist1);  
    
    std::string bbuffer = sp.read(10240);

    //分割收到的数据
    auto res=vStringSplit(bbuffer);

    //joint err t1~6 and t7~8=0
    int jointErr[]={t1,t2,t3,t4,t5,t6,0,0};

    serial_demo::ti5_motorStatus motor_stat;
    
	for(int i=0;i<res.size();i++)
	{
        if(res[i].size()>140)
        {
            std::string readBuffer = res[i];
            int index = readBuffer.size();
            uint8_t mem[400]={0}, memIndex=0;
           
            int nOfCanMsg = 0;//收到CAN消息的个数
            if(readBuffer.at(0)=='<' )
            {
                std::string numOfCanMsg = "00";
                numOfCanMsg.at(0) = readBuffer.at(1);
                numOfCanMsg.at(1) = readBuffer.at(2);
                nOfCanMsg = translate(numOfCanMsg,2);
            }
            else
            {
                std::cout<<"***************first char is not '<'***************"<<std::endl;
                continue;
            }
            //6个电机143，7个电机163
            int nOfChar = (1+(1+nOfCanMsg*2+8*nOfCanMsg)*2);//收到字符的个数
            if(index==nOfChar)//the data from serialPort ,the first CHAR is "<"
            {	
                std::cout<<"OriBuffer:"<<readBuffer<<std::endl;
                std::cout<<"***nOfCanMsg:"<<nOfCanMsg<<"***nOfChar:"<<nOfChar<<std::endl;

                char line[nOfChar];
                strncpy(line,readBuffer.c_str(),readBuffer.length()+1);

                char I[4],V[4],S[8];
                int Iv,Vv,Sv;
                std::string inv1;
                std::string inv2;
                
                int k=27;
                if(nOfCanMsg==6)
                {
                    k=27;
                }
                else if(nOfCanMsg==7)
                {
                    k=31;
                }
                else if(nOfCanMsg==8)
                {
                    k=35;
                }
                else
                {
                    std::cout<<"*****nOfCanMsg:" <<nOfCanMsg<<"***are not 6 or 7 or 8 *******"<<std::endl;
                    continue;
                }

                double IList[10],VList[10],SList[10];

                for (int ii=0;ii<nOfCanMsg-1;ii++)
                {
                    getdata(line,I,k+ii*16,k+ii*16+4);
                    getdata(line,V,k+ii*16+4,k+ii*16+8);
                    getdata(line,S,k+ii*16+8,k+ii*16+16);
                    // std::cout<<"***getdataI:" <<I<<"***getdataV:" <<V<<"***getdataIS:"<<S<<std::endl;
                    ivs(I,inv1,4);
                    // std::cout<<"*****invI:" <<inv1.data()<<"******";
                    Iv=u2int(translate(inv1,4));
                    // std::cout<<"*****translateI:" <<translate(inv1,4)<<"*****u2intI:" <<Iv<<"******"<<std::endl;

                    ivs(V,inv1,4);
                    // std::cout<<"*****invV:" <<inv1.data()<<"******";
                    Vv=u2int(translate(inv1,4));
                    // std::cout<<"*****translateV:" <<translate(inv1,4)<<"*****u2intV:" <<Vv<<"******"<<std::endl;
                    ivs(S,inv2,8);
                    // std::cout<<"*****invS:" <<inv2.data()<<"******";
                    Sv=(int)translate(inv2,8);
                    // if (Sv<0)
                    // {
                    //     // std::cout<<"*****Sv<0,sv:" <<Sv<<"******"<<std::endl;
                    //     Sv=(int)translate(inv2,8);
                    //     // std::cout<<"*****translateS:" <<translate(inv2,8)<<"*****intS:" <<Sv<<"******"<<std::endl;
                    // }
                    // else
                    // {
                    //     // std::cout<<"*****translateS:" <<translate(inv2,8)<<"*****intS:" <<Sv<<"******"<<std::endl;
                    // }
                    double Vvv = (Vv/101.0)*3.6;//转化为度每秒公式：(返回值/100/减速比)*360
                    //减去关节零位误差
                    Sv -= jointErr[ii];
                    if(ii==2||ii==4)//3轴 5轴 是反转
                    {
                        Sv = -Sv;
                    }
                    double Svv = (((Sv)/6553.6)/101)*36;//转化为减速机角度公式：(返回值/65536/减速比)*360
                    if(ii==4||ii==5)
                    {
                        Svv = Svv/0.51;
                    }
                    cout<<"ID:"<<ii+1<<" I:"<<Iv<<" v:"<<Vvv<<" s:"<<Svv<<endl;

                    IList[ii] = Iv;
                    VList[ii] = Vvv;
                    SList[ii] = Svv;

                    motor_stat.joint1_7DegLists.push_back(Svv);
                    motor_stat.velocityList.push_back(Vvv); 
                    motor_stat.eleList.push_back(IList[ii]);

                    *(poseList+ii) = SList[ii];
                    
                }//for (int ii=0;ii<nOfCanMsg;ii++)
                motor_stat.numOfMotor = nOfCanMsg-1;
                motor_stat.header.stamp = ros::Time::now();
                motor_stat.mode_uint = 0;
                this->motorPub.publish(motor_stat);
            }//end if(index==nOfChar)
            else
            {
                std::cout<<"****readBuff index: "<<index<<"are not caculated  nOfChar："<<nOfChar<<std::endl;
            }
        }//end if(res[i].size()>140)
    }// end for(int i=0;i<res.size();i++)

    return 0;
}
 



//subscribe joint msg from ti5_targetPos and send joint cmd to ti5_arm
void Listener::callback(const serial_demo::ti5_polyJC_Lists::ConstPtr &msg)
 {
    // int n = msg->trajectory[0].joint_trajectory.points.size();
    
    double next_position[6]={0};

    double ori_position[6]={0} ;
    double cha_position[6]={0} ;
    double now_position[6]={0} ;

    double ori_gripper_pos=0 ;
    double next_gripper_pos=0 ;
    int gripper_pos_flag = 0;

    bool flag_n = 0;
    //控制周期/微妙
    double JcmdPeriod = 10000;

    //下一位姿与当前位姿的关节角差值
    int cn[6];

    //下一位姿与当前位姿的关节角差值的绝对值
    int abscn[6];
    //下一位姿与当前位姿的关节角的时间序列差
    double cnTime = 0;

    char str[5120];
    char str123[5120];
    float ww;
    float vv;
    float zs;
    float zs1;
    float zss =1.0;
    std::string end_position ="";
    
    if(msg->cmdRate <100 || msg->cmdRate>1000)
    {
        std::cout<<"*************++++++++++++++cmdRate are not in[100,1000]:"<<msg->cmdRate<< "+++++++++++******************" <<std::endl;
        return;
    }
    JcmdPeriod = 1000000/msg->cmdRate;
    std::cout<<"*************++++++++++++++cmdPeriod:"<<JcmdPeriod<<"++++++++++------******"<<endl;
    if(msg->numOfCmd==6||msg->numOfCmd==7)
    {
        int n = msg->joint1RadList.size();//注意：所有关节角的数组长度因该一致

        if(n!=msg->joint2RadList.size())
        {flag_n = 1;}
        if(n!=msg->joint3RadList.size())
        {flag_n = 1;}
        if(n!=msg->joint4RadList.size())
        {flag_n = 1;}
        if(n!=msg->joint5RadList.size())
        {flag_n = 1;}
        if(n!=msg->joint6RadList.size())
        {flag_n = 1;}
        if(msg->numOfCmd==7)
        {
        
            if(n!=msg->joint7RadList.size())
            {flag_n = 1;}
            
        }
        if(msg->interpolyReq!=0 && n != msg->timeLists.size())
        {flag_n = 1;}
        if(flag_n)
        {
            std::cout<<"the dimension of jointRadList/timeList are different"<<std::endl;
            return;
        }

        if(msg->interpolyReq == 1) //使用默认插值插值
        {
            std::cout<<"**+++++++++Listener::callback called sendCmd start++++++++++***"<<endl;
            for (int i = 0; i < n-1; i++)
            {
                

                ori_position[0]=msg->joint1RadList[i]*degTrans ;
                ori_position[1]=msg->joint2RadList[i]*degTrans ;
                ori_position[2]=msg->joint3RadList[i]*degTrans ;
                ori_position[3]=msg->joint4RadList[i]*degTrans ;
                ori_position[4]=msg->joint5RadList[i]*degTrans ;
                ori_position[5]=msg->joint6RadList[i]*degTrans ;
                if(msg->numOfCmd==7)
                {ori_gripper_pos =msg->joint7RadList[i];}
                
                
                next_position[0]=msg->joint1RadList[i+1]*degTrans ;
                next_position[1]=msg->joint2RadList[i+1]*degTrans ;
                next_position[2]=msg->joint3RadList[i+1]*degTrans ;
                next_position[3]=msg->joint4RadList[i+1]*degTrans ;
                next_position[4]=msg->joint5RadList[i+1]*degTrans ;
                next_position[5]=msg->joint6RadList[i+1]*degTrans ;
                if(msg->numOfCmd==7)
                {next_gripper_pos = msg->joint7RadList[i+1];}
                

                cn[0]=next_position[0]-ori_position[0];
                cn[1]=next_position[1]-ori_position[1];
                cn[2]=next_position[2]-ori_position[2];
                cn[3]=next_position[3]-ori_position[3];
                cn[4]=next_position[4]-ori_position[4];
                cn[5]=next_position[5]-ori_position[5];

                if(next_gripper_pos!=ori_gripper_pos)
                {
                    gripper_pos_flag = 1;//gripper will act
                    next_gripper_pos = 0;
                    ori_gripper_pos = 0;
                }
                else
                {
                    gripper_pos_flag = 0;//gripper will not act
                }

                for(int mmm=0; mmm<6;mmm++)
                {
                    abscn[mmm] = abs(cn[mmm]);
                }

                int round_i;//插补步数

                //状态标志 round_i>100时 status=1
                int status =0;

                if ((abscn[0]<1000) && (abscn[1]<1000) && (abscn[2]<1000) && (abscn[3]<10000) && (abscn[4]<10000) && (abscn[5]<10000))
                {
                    round_i=20/2;
                }
                else if(abscn[0]<10000 && abscn[1]<10000 && abscn[2]<10000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
                {
                    round_i=100/2;
                }

                else if(abscn[0]<50000 && abscn[1]<50000 && abscn[2]<50000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
                {
                    round_i=200/2;
                }

                else if(abscn[0]<100000 && abscn[1]<100000 && abscn[2]<100000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
                {round_i=500/2;
                status=1;
                }

                else if(abscn[0]<500000 && abscn[1]<500000 && abscn[2]<500000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
                {round_i=1000/2;
                status=1;
                }
                else if(abscn[0]<10000000 && abscn[1]<10000000 && abscn[2]<10000000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
                {round_i=2000/2;
                status=1;
                }

                else {
                round_i=20/2;
                }
        
                //std::cout<<"round_i"<<round_i<<std::endl;

                //关节角度
                int    r1[500]={0};
                int    r2[500]={0};
                int    r3[500]={0};
                int    r4[500]={0};
                int    r5[500]={0};
                int    r6[500]={0};

                //参数列表
                uint32_t parameterlist2[10]={0};

                //单步插补绝对值
                int stepValue[6] = {};

                for(int mmmm=0; mmmm<6;mmmm++)
                {
                    stepValue[mmmm] = abscn[mmmm]/round_i;
                }

                for (int i=0;i<round_i;i++)
                {            
                    if (status==1&&10<i<round_i*0.7)//状态=1   且10<i<插补数的7成，插补跳10步
                    {i=i+10;}
                    if (10<i<round_i*0.7)//10<i<插补数的7成，插补不跳步
                    {i=i;}
                
                                if (cn[0]<=0)
                                {
                                    r1[i]=ori_position[0] - (i*stepValue[0]);
                                }
                                else if(cn[0]>0)
                                {
                                    r1[i]=ori_position[0] + (i*stepValue[0]);
                                }

                                if (cn[1]<=0)
                                {
                                    r2[i]=ori_position[1] - (i*stepValue[1]);
                                }
                                else if(cn[1]>0)
                                {
                                    r2[i]=ori_position[1] + (i*stepValue[1]);
                                }

                                if (cn[2]<=0)
                                {
                                    r3[i]=ori_position[2] - (i*stepValue[2]);
                                }
                                else if(cn[2]>0)
                                {
                                    r3[i]=ori_position[2] + (i*stepValue[2]);
                                }


                                if (cn[3]<=0)
                                {
                                    r4[i]=ori_position[3] - (i*stepValue[3]);
                                }
                                else if(cn[3]>0)
                                {
                                    r4[i]=ori_position[3] + (i*stepValue[3]);
                                }

                                if (cn[4]<=0)
                                {
                                    r5[i]=(ori_position[4] - (i*stepValue[4]))*0.51;
                                }
                                else if(cn[4]>0)
                                {
                                    r5[i]=(ori_position[4] + (i*stepValue[4]))*0.51;
                                }

                                if (cn[5]<=0)
                                {
                                    r6[i]=(ori_position[5] - (i*stepValue[5]))*0.51;
                                }
                                else if(cn[5]>0)
                                {
                                    r6[i]=(ori_position[5] + (i*stepValue[5]))*0.51;
                                }

                                parameterlist2[0] = r1[i]+t1;
                                parameterlist2[1] = r2[i]+t2;
                                parameterlist2[2] = -r3[i]+t3;
                                parameterlist2[3] = r4[i]+t4;
                                parameterlist2[4] = -r5[i]+t5;
                                parameterlist2[5] = r6[i]+t6;

                    //CAN ID号，命令列表0x1E(30)表示位置控制
                    uint8_t canidlist2[10]={1,2,3,4,5,6},cmd2[10]={30,30,30,30,30,30};

                    // uint32_t parameterlist2[10]={ori_position[0]/6.28*101*65536,ori_position[1]/6.28*101*65536,ori_position[2]/6.28*101*65536};
                
                    //发送命令和参数
                    // std::cout<<"**+++++++++Listener::callback called sendCmd++++++++++***"<<endl;
                    usleep(3000);
                    sendCommand(6,canidlist2,cmd2,parameterlist2);  

                }//end for(int i=0;i<round_i;i++)
                uint32_t parameterlist3[10]={0};
                parameterlist3[0] = int(next_position[0])+t1;
                parameterlist3[1] = int(next_position[1])+t2;
                parameterlist3[2] = -int(next_position[2])+t3;
                parameterlist3[3] = int(next_position[3])+t4;
                parameterlist3[4] = -int(next_position[4]*0.51)+t5;
                parameterlist3[5] = int(next_position[5]*0.51)+t6;
                //CAN ID号，命令列表0x1E(30)表示位置控制
                uint8_t canidlist3[10]={1,2,3,4,5,6},cmd3[10]={30,30,30,30,30,30};

                //发送命令和参数
                std::cout<<"**+++++++++Listener::callback called ++last++ sendCmd++++++++++***"<<endl;
                usleep(3000);
                sendCommand(6,canidlist3,cmd3,parameterlist3);
                if(gripper_pos_flag>=1)//gripper will act
                {
                    uint32_t parameterlist4[10]={0};
                    uint8_t canidlist4[10]={7},cmd4[10]={30};
                    parameterlist4[0] = int(next_gripper_pos);
                    usleep(3000);
                    sendCommand(1,canidlist4,cmd4,parameterlist4);
                }

            }//end for(int i = 1; i < n-1; i++)
            std::cout<<"**+++++++++Listener::callback called sendCmd end++++++++++***"<<endl;
        }//end if(msg->interpolyReq == 1)
        else if(msg->interpolyReq==0) // 外部插值直接控制6/7个电机
        {
            std::cout<<"**+++++++++Listener::callback (external control directly) called sendCmd start++++++++++***"<<endl;
            double time_do_msg = ros::Time::now().toNSec();
            double time_down_msg = ros::Time::now().toSec();
            if(msg->numOfCmd==7)//7个电机
            {
                for (int i = 0; i < n; i++)
                {
                    next_position[0]=msg->joint1RadList[i]*degTrans_poly ;
                    next_position[1]=msg->joint2RadList[i]*degTrans_poly ;
                    next_position[2]=msg->joint3RadList[i]*degTrans_poly ;
                    next_position[3]=msg->joint4RadList[i]*degTrans_poly ;
                    next_position[4]=msg->joint5RadList[i]*degTrans_poly ;
                    next_position[5]=msg->joint6RadList[i]*degTrans_poly ;
                    next_gripper_pos = msg->joint7RadList[i];

                    uint32_t parameterlist5[10]={0};
                    parameterlist5[0] = int(next_position[0])+t1;
                    parameterlist5[1] = int(next_position[1])+t2;
                    parameterlist5[2] = -int(next_position[2])+t3;
                    parameterlist5[3] = int(next_position[3])+t4;
                    parameterlist5[4] = -int(next_position[4]*0.51)+t5;
                    parameterlist5[5] = int(next_position[5]*0.51)+t6;
                    parameterlist5[6] = int(next_gripper_pos);
                    //CAN ID号，命令列表0x1E(30)表示位置控制
                    uint8_t canidlist5[10]={1,2,3,4,5,6,7},cmd5[10]={30,30,30,30,30,30,30};
                    // std::cout<<"**+++++++++Listener::callback (external control directly) called sendCmd++++++++++***"<<endl;
                    sendCommand(7,canidlist5,cmd5,parameterlist5);
                    usleep(JcmdPeriod);//100-1000Hz
                }
            }
            else if(msg->numOfCmd==6)//6个电机
            {
                for (int i = 0; i < n; i++)
                {
                    next_position[0]=msg->joint1RadList[i]*degTrans_poly ;
                    next_position[1]=msg->joint2RadList[i]*degTrans_poly ;
                    next_position[2]=msg->joint3RadList[i]*degTrans_poly ;
                    next_position[3]=msg->joint4RadList[i]*degTrans_poly ;
                    next_position[4]=msg->joint5RadList[i]*degTrans_poly ;
                    next_position[5]=msg->joint6RadList[i]*degTrans_poly ;
                
                    uint32_t parameterlist5[10]={0};
                    parameterlist5[0] = int(next_position[0])+t1;
                    parameterlist5[1] = int(next_position[1])+t2;
                    parameterlist5[2] = -int(next_position[2])+t3;
                    parameterlist5[3] = int(next_position[3])+t4;
                    parameterlist5[4] = -int(next_position[4]*0.51)+t5;
                    parameterlist5[5] = int(next_position[5]*0.51)+t6;
                 
                    //CAN ID号，命令列表0x1E(30)表示位置控制
                    uint8_t canidlist5[10]={1,2,3,4,5,6},cmd5[10]={30,30,30,30,30,30};
                    // std::cout<<"**+++++++++Listener::callback (external control directly) called sendCmd++++++++++***"<<endl;
                    sendCommand(6,canidlist5,cmd5,parameterlist5);
                    usleep(JcmdPeriod);//100-1000Hz
                }
            }
            else
            {std::cout<<"error!, msg->numOfCmd is not 6 nither 7"<<std::endl;}
            time_down_msg = ros::Time::now().toNSec();
            std::cout<<"++++++++++++++++++use NSectime:"<< time_down_msg - time_do_msg <<"+++++++++++++++++++++"<<std::endl;
            std::cout<<"**+++++++++Listener::callback (external control directly) called sendCmd end++++++++++***"<<endl;
        }//else if(msg->interpolyReq==0)
        else if(msg->interpolyReq==10) //内部线性插值
        {
            
            double time_do_msg = ros::Time::now().toNSec();
            double time_down_msg = ros::Time::now().toSec();
            //关节角度
            int    r1[50000]={0};
            int    r2[50000]={0};
            int    r3[50000]={0};
            int    r4[50000]={0};
            int    r5[50000]={0};
            int    r6[50000]={0};
            
            //参数列表
            uint32_t parameterlistLpoly[10]={0};

            for (int i = 0; i < n-1; i++)
            {
                ori_position[0]=msg->joint1RadList[i]*degTrans ;
                ori_position[1]=msg->joint2RadList[i]*degTrans ;
                ori_position[2]=msg->joint3RadList[i]*degTrans ;
                ori_position[3]=msg->joint4RadList[i]*degTrans ;
                ori_position[4]=msg->joint5RadList[i]*degTrans ;
                ori_position[5]=msg->joint6RadList[i]*degTrans ;
                if(msg->numOfCmd==7)
                {
                    ori_gripper_pos =msg->joint7RadList[i];
                    next_gripper_pos = msg->joint7RadList[i+1];
                }
                
                next_position[0]=msg->joint1RadList[i+1]*degTrans ;
                next_position[1]=msg->joint2RadList[i+1]*degTrans ;
                next_position[2]=msg->joint3RadList[i+1]*degTrans ;
                next_position[3]=msg->joint4RadList[i+1]*degTrans ;
                next_position[4]=msg->joint5RadList[i+1]*degTrans ;
                next_position[5]=msg->joint6RadList[i+1]*degTrans ;
                
                if(next_gripper_pos!=ori_gripper_pos)
                {
                    gripper_pos_flag = 1;//gripper will act
                }

                cn[0]=next_position[0]-ori_position[0];
                cn[1]=next_position[1]-ori_position[1];
                cn[2]=next_position[2]-ori_position[2];
                cn[3]=next_position[3]-ori_position[3];
                cn[4]=next_position[4]-ori_position[4];
                cn[5]=next_position[5]-ori_position[5];

                cnTime = msg-> timeLists[i+1] - msg->timeLists[i];
                double UScnTime = cnTime*1000000;
                int LinterpolyStep = int(UScnTime/JcmdPeriod);
                int LpolyStepValue[6]={0};
                for(int i = 0; i<6; i++)
                {
                    LpolyStepValue[i] = cn[i]/LinterpolyStep;
                }
                std::cout<<"++++++++++++++++++iner Linearinterpoly sendcmd start+++++++++++++++"<< JcmdPeriod<<"**"<<LinterpolyStep <<std::endl;
                for(int i=0; i<LinterpolyStep; i++)
                {
                    r1[i] = ori_position[0] + (i+1)*LpolyStepValue[0];
                    r2[i] = ori_position[1] + (i+1)*LpolyStepValue[1];
                    r3[i] = ori_position[2] + (i+1)*LpolyStepValue[2];
                    r4[i] = ori_position[3] + (i+1)*LpolyStepValue[3];
                    r5[i] = (ori_position[4] + (i+1)*LpolyStepValue[4])*0.51;
                    r6[i] = (ori_position[5] + (i+1)*LpolyStepValue[5])*0.51;
                   
                    parameterlistLpoly[0] = r1[i]+t1;
                    parameterlistLpoly[1] = r2[i]+t2;
                    parameterlistLpoly[2] = -r3[i]+t3;
                    parameterlistLpoly[3] = r4[i]+t4;
                    parameterlistLpoly[4] = -r5[i]+t5;
                    parameterlistLpoly[5] = r6[i]+t6;
                    uint8_t canidlistLpoly[10]={1,2,3,4,5,6};
                    uint8_t cmdLpoly[10]={30,30,30,30,30,30};
                    
                    // std::cout<<"++++++++++++++++++iner Linearinterpoly sendcmd+++++++++++++++"<< JcmdPeriod<<"**"<<LinterpolyStep <<std::endl;
                    sendCommand(6,canidlistLpoly,cmdLpoly,parameterlistLpoly);  
                    usleep(JcmdPeriod);
                }//end (i=0; i<LinterpolyStep; i++)
                if(gripper_pos_flag>=1)
                {
                    uint32_t parameterlistG[10]={0};
                    uint8_t canidlistG[10]={7},cmdG[10]={30};
                    parameterlistG[0] = int(next_gripper_pos);
                    // usleep(3000);
                    sendCommand(1,canidlistG,cmdG,parameterlistG);
                }

            }// end for (int i = 0; i < n-1; i++)

            time_down_msg = ros::Time::now().toNSec();
            std::cout<<"++++++++++++++++++use NSectime:"<< time_down_msg - time_do_msg <<"+++++++++++++++++++++"<<std::endl;
            std::cout<<"++++++++++++++++++iner Linearinterpoly sendcmd end+++++++++++++++"<<std::endl;
        }//else if(msg->interpolyReq==10)
        else //no interpolation describe by msg->interpolyReq
        {
            std::cout<<"No interpolation algorithm found for requirment:msg->interpolyReq="<<msg->interpolyReq<<std::endl;
            return;
        }
    }//end if(msg->numOfCmd==6||msg->numOfCmd==7)
    else if(msg->numOfCmd == 1)//only gripper cmd
    {
        
        uint32_t parameterlist7[10]={0};
        uint8_t canidlist7[10]={7},cmd7[10]={30};
        parameterlist7[0] = int(msg->joint7RadList[0]);
        std::cout<<"**+++++++++Listener::callback only gripper called sendCmd++++++++++***"<<endl;
        sendCommand(1,canidlist7,cmd7,parameterlist7);
        
    }


}//end Listener::callback

//get joint msg from struct jsonData and send joint cmd to ti5_arm
void Listener::socketCcallback(const jsonData jData)
 {
    // int n = msg->trajectory[0].joint_trajectory.points.size();
    double next_position[6]={0};

    double ori_position[6]={0} ;
    double cha_position[6]={0} ;
    double now_position[6]={0} ;
    double ori_gripper_pos=0 ;
    double next_gripper_pos=0 ;
   

    int gripper_pos_flag = 0;


    bool flag_n = 0;
    int n = jData.joint1.size();//注意：所有关节角的数组长度因该一致

    if(n!=jData.joint2.size())
    {flag_n = 1;}
    if(n!=jData.joint3.size())
    {flag_n = 1;}
    if(n!=jData.joint4.size())
    {flag_n = 1;}
    if(n!=jData.joint5.size())
    {flag_n = 1;}
    if(n!=jData.joint6.size())
    {flag_n = 1;}
    if(n!=jData.gripper.size())
    {flag_n = 1;}
    if(n!=jData.time.size())
    {flag_n = 1;}

    if(flag_n)
    {
        std::cout<<"the dimension of jointRadList are different"<<std::endl;
        return;
    }


    //下一位姿与当前位姿的关节角差值
    int cn[6];

    //下一位姿与当前位姿的关节角差值的绝对值
    int abscn[6];

    char str[5120];
    char str123[5120];
    float ww;
    float vv;
    float zs;
    float zs1;
    float zss =1.0;
    std::string end_position ="";

    for (int i = 0; i < n-1; i++)
    {
		usleep(3000);
     
    
        ori_position[0]=jData.joint1[i] *degTrans ;
        ori_position[1]=jData.joint2[i] *degTrans ;
        ori_position[2]=jData.joint3[i] *degTrans ;
        ori_position[3]=jData.joint4[i] *degTrans ;
        ori_position[4]=jData.joint5[i] *degTrans ;
        ori_position[5]=jData.joint6[i] *degTrans ;
        ori_gripper_pos = jData.gripper[i];

        next_position[0]=jData.joint1[i+1] *degTrans ;
        next_position[1]=jData.joint2[i+1] *degTrans ;
        next_position[2]=jData.joint3[i+1] *degTrans ;
        next_position[3]=jData.joint4[i+1] *degTrans ;
        next_position[4]=jData.joint5[i+1] *degTrans ;
        next_position[5]=jData.joint6[i+1] *degTrans ;
        next_gripper_pos = jData.gripper[i+1];

        cn[0]=next_position[0]-ori_position[0];
        cn[1]=next_position[1]-ori_position[1];
        cn[2]=next_position[2]-ori_position[2];
        cn[3]=next_position[3]-ori_position[3];
        cn[4]=next_position[4]-ori_position[4];
        cn[5]=next_position[5]-ori_position[5];
        
        if(next_gripper_pos!=ori_gripper_pos)
        {
            gripper_pos_flag = 1;//gripper will act
        }
        else
        {
            gripper_pos_flag = 0;//gripper will not act
        }

        for(int mmm=0; mmm<6;mmm++)
        {
            abscn[mmm] = abs(cn[mmm]);
        }

        int round_i;//插补步数

        //状态标志 round_i>100时 status=1
	    int status =0;

        if ((abscn[0]<1000) && (abscn[1]<1000) && (abscn[2]<1000) && (abscn[3]<10000) && (abscn[4]<10000) && (abscn[5]<10000))
        {
            round_i=20/2;
        }
        else if(abscn[0]<10000 && abscn[1]<10000 && abscn[2]<10000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
        {
            round_i=100/2;
	    }

        else if(abscn[0]<50000 && abscn[1]<50000 && abscn[2]<50000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
        {
            round_i=200/2;
	    }

        else if(abscn[0]<100000 && abscn[1]<100000 && abscn[2]<100000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
        {round_i=500/2;
		status=1;
        }

        else if(abscn[0]<500000 && abscn[1]<500000 && abscn[2]<500000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
        {round_i=1000/2;
		status=1;
        }
        else if(abscn[0]<10000000 && abscn[1]<10000000 && abscn[2]<10000000 && abscn[3]<10000 && abscn[4]<10000 && abscn[5]<10000)
        {round_i=2000/2;
		status=1;
        }

        else {
        round_i=20/2;
        }
 

        //std::cout<<"round_i"<<round_i<<std::endl;


        //关节角度
        int    r1[500]={0};
        int    r2[500]={0};
        int    r3[500]={0};
        int    r4[500]={0};
        int    r5[500]={0};
        int    r6[500]={0};

        //参数列表
	    uint32_t parameterlist2[10]={0};

        //单步插补绝对值
        int stepValue[6] = {};

        for(int mmmm=0; mmmm<6;mmmm++)
        {
            stepValue[mmmm] = abscn[mmmm]/round_i;
        }

        for (int i=0;i<round_i;i++)
        {            
            if (status==1&&10<i<round_i*0.7)//状态=1   且10<i<插补数的7成，插补跳10步
            {i=i+10;}
            if (10<i<round_i*0.7)//10<i<插补数的7成，插补不跳步
            {i=i;}
          
                         if (cn[0]<=0)
                         {
                             r1[i]=ori_position[0] - (i*stepValue[0]);
                         }
                         else if(cn[0]>0)
                         {
                             r1[i]=ori_position[0] + (i*stepValue[0]);
                         }

                         if (cn[1]<=0)
                         {
                             r2[i]=ori_position[1] - (i*stepValue[1]);
                         }
                         else if(cn[1]>0)
                         {
                             r2[i]=ori_position[1] + (i*stepValue[1]);
                         }

                         if (cn[2]<=0)
                         {
                             r3[i]=ori_position[2] - (i*stepValue[2]);
                         }
                         else if(cn[2]>0)
                         {
                             r3[i]=ori_position[2] + (i*stepValue[2]);
                         }


                         if (cn[3]<=0)
                         {
                             r4[i]=ori_position[3] - (i*stepValue[3]);
                         }
                         else if(cn[3]>0)
                         {
                             r4[i]=ori_position[3] + (i*stepValue[3]);
                         }

                         if (cn[4]<=0)
                         {
                            //
                             r5[i]=(ori_position[4] - (i*stepValue[4]))*0.51;
                         }
                         else if(cn[4]>0)
                         {
                             r5[i]=(ori_position[4] + (i*stepValue[4]))*0.51;
                         }

                         if (cn[5]<=0)
                         {
                             r6[i]=(ori_position[5] - (i*stepValue[5]))*0.51;
                         }
                         else if(cn[5]>0)
                         {
                             r6[i]=(ori_position[5] + (i*stepValue[5]))*0.51;
                         }

                        parameterlist2[0] = r1[i]+t1;
                        parameterlist2[1] = r2[i]+t2;
                        parameterlist2[2] = -r3[i]+t3;
                        parameterlist2[3] = r4[i]+t4;
                        parameterlist2[4] = -r5[i]+t5;
                        parameterlist2[5] = r6[i]+t6;

            //CAN ID号，命令列表0x1E(30)表示位置控制
            uint8_t canidlist2[10]={1,2,3,4,5,6},cmd2[10]={30,30,30,30,30,30};

            //发送命令和参数
            std::cout<<"**+++++++++socketC callback called sendCmd++++++++++***"<<endl;
            sendCommand(6,canidlist2,cmd2,parameterlist2);
  
        }//end for(int i=0;i<round_i;i++)
        uint32_t parameterlist3[10]={0};
        parameterlist3[0] = int(next_position[0])+t1;
        parameterlist3[1] = int(next_position[1])+t2;
        parameterlist3[2] = -int(next_position[2])+t3;
        parameterlist3[3] = int(next_position[3])+t4;
        parameterlist3[4] = -int(next_position[4]*0.51)+t5;
        parameterlist3[5] = int(next_position[5]*0.51)+t6;
        //CAN ID号，命令列表0x1E(30)表示位置控制
        uint8_t canidlist3[10]={1,2,3,4,5,6},cmd3[10]={30,30,30,30,30,30};

        //发送命令和参数
        std::cout<<"**+++++++++socketC callback called ++last++ sendCmd++++++++++***"<<endl;
        sendCommand(6,canidlist3,cmd3,parameterlist3);
        if(gripper_pos_flag>=1)//gripper will act
        {
            uint32_t parameterlist4[10]={0};
            uint8_t canidlist4[10]={7},cmd4[10]={30};
            parameterlist4[0] = int(next_gripper_pos);
            sendCommand(1,canidlist4,cmd4,parameterlist4);
        }
       

        // for(int klk=0;klk<6;klk++)
        // {
        //     std::cout<<"*parameterlist3"<<klk<<":"<<int(parameterlist3[klk])<<endl;
        // }



    }//end for(int i = 1; i < n-1; i++)

}//end Listener::callback


/*回零*/
void homing(double* poseList)
{

    double next_position[6]={0};
    double ori_position[6]={0} ;

    //下一位姿与当前位姿的关节角差值
    int cn[6];

    //关节角度
    int    r1=0;
    int    r2=0;
    int    r3=0;
    int    r4=0;
    int    r5=0;
    int    r6=0;
    

        ori_position[0]=(*(poseList+0)) * degTrans ;
        ori_position[1]=(*(poseList+1)) * degTrans ;
        ori_position[2]=(*(poseList+2)) * degTrans ;
        ori_position[3]=(*(poseList+3)) * degTrans ;
        ori_position[4]=(*(poseList+4)) * degTrans ;
        ori_position[5]=(*(poseList+5)) * degTrans ;
        
        next_position[0]=0 ;
        next_position[1]=0 ;
        next_position[2]=0 ;
        next_position[3]=0 ;
        next_position[4]=0 ;
        next_position[5]=0 ;
       
        cn[0]=next_position[0]-ori_position[0];
        cn[1]=next_position[1]-ori_position[1];
        cn[2]=next_position[2]-ori_position[2];
        cn[3]=next_position[3]-ori_position[3];
        cn[4]=next_position[4]-ori_position[4];
        cn[5]=next_position[5]-ori_position[5];

        
        //控制周期/微妙 //500Hz
        double JcmdPeriod = 2000;
        double UStime = 6*1000000;//回零时间固定6s
        int LinterpolyStep = int(UStime/JcmdPeriod);//步数
        int LpolyStepValue[6] = {0};//步长
        for(int i=0;i<6;i++)
        {
            LpolyStepValue[i] = int(cn[i]/LinterpolyStep);
        }
        
        //参数列表
        uint32_t parameterlistLpoly[10]={0};
       
        std::cout<<"homing......"<<std::endl;

        for(int i=0; i<LinterpolyStep; i++)
        {
            r1 = ori_position[0] + (i+1)*LpolyStepValue[0];
            r2 = ori_position[1] + (i+1)*LpolyStepValue[1];  
            r3 = ori_position[2] + (i+1)*LpolyStepValue[2];
            r4 = ori_position[3] + (i+1)*LpolyStepValue[3]; 
            r5 = ori_position[4] + (i+1)*LpolyStepValue[4];
            r6 = ori_position[5] + (i+1)*LpolyStepValue[5]; 
           
            parameterlistLpoly[0] = r1+t1;
            parameterlistLpoly[1] = r2+t2;
            parameterlistLpoly[2] = -r3+t3;
            parameterlistLpoly[3] = r4+t4;
            parameterlistLpoly[4] = -(r5*0.51)+t5;
            parameterlistLpoly[5] = (r6*0.51)+t6;
            uint8_t canidlistLpoly[10] = {1,2,3,4,5,6};
            uint8_t cmdLpoly[10]={30,30,30,30,30,30};
            usleep(JcmdPeriod);
            sendCommand(6,canidlistLpoly,cmdLpoly,parameterlistLpoly);

        }


            parameterlistLpoly[0] = t1;
            parameterlistLpoly[1] = t2;
            parameterlistLpoly[2] = t3;
            parameterlistLpoly[3] = t4;
            parameterlistLpoly[4] = t5;
            parameterlistLpoly[5] = t6;
            parameterlistLpoly[6] = 0;
            uint8_t canidlistLpoly[10] = {1,2,3,4,5,6,7};
            uint8_t cmdLpoly[10]={30,30,30,30,30,30,30};
        usleep(3000);
        sendCommand(1,canidlistLpoly,cmdLpoly,parameterlistLpoly);
        std::cout<<"homed!"<<std::endl;
} //end homing()


int main(int argc, char** argv)
{
    ros::init(argc, argv, "interPolyPoint_Sub_node");
    Listener listener;
    //ros::Rate loop_rate(MESSAGE_FREQ); // set the rate as defined in the macro MESSAGE_FREQ
    ros::Rate loop_rate(2000);//2000Hz
    ros::Rate loop_rate_slow(2);//2Hz
    
    ros::Subscriber client_sub = listener.nh.subscribe("interPolyPoint_topic", 1000, &Listener::callback, &listener);
    
    //不连接matlab 将下列代码注释
    /*++++++++++++++++++++++++++++连接matlab++++++++++++++++++++++++*/
    // if(-1 == listener.createSocketClient())
    // {
      
    //     std::cout<<"createSocketClient failed!"<<std::endl;
    //     return -1;
    // }
    // std::cout<<"socket Client creat successfully!"<<std::endl;
    /*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

    //创建一个serial对象 (全局已创建)
    //创建timeout
    serial::Timeout to = serial::Timeout::simpleTimeout(100);
    //设置要打开的串口名称
    sp.setPort("/dev/ttyUSB0");
    //设置串口通信的波特率
    sp.setBaudrate(2000000);
    //串口设置timeout
    sp.setTimeout(to);
 
    try
    {
        //打开串口
        sp.open();
    }
    catch(serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }
    
    //判断串口是否打开成功
    if(sp.isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return -1;
    }

	for(int i=0;i<3;i++)//设置电机为停止模式
    {    
        uint8_t canidlist[10]={0},cmd[10]={2};
        uint32_t parameterlist[10]={0};
        sendCommand(0,canidlist,cmd,parameterlist);  
        loop_rate_slow.sleep();
    }
	for(int ii=0;ii<3;ii++)//获取电机电流
    {    
        uint8_t canidlist1[10]={0},cmd1[10]={4};
        uint32_t parameterlist1[10]={0};
        sendCommand(0,canidlist1,cmd1,parameterlist1);  
        loop_rate_slow.sleep();
    }
    

    //存放读取的位置
    double poseList[8] = {0}; 
/********************** homing if usr input param[1] is 'home' *************************/
    std::cout<<"argc:"<<argc<< std::endl;
    if(argc>=2)
    {
        std::cout<<"argv:";
        for(int i=0; i<argc; i++)
        {
            std::cout<<argv[i]<<" ";
        }
        std::cout<<std::endl;

        if(argv[1] == std::string ("home"))
        {
            double lastPoseList[8] = {0};
            int count = 0;
            while(count<=5)
            {
                try
                {
                    std::cout<<"count:"<<count<<std::endl;
                    loop_rate_slow.sleep();  

                    //get joints' I V and S              
                    listener.readCommand(poseList);
                    
                    bool poseFlag = 0;
                    for(int i=0; i<7; i++)
                    {
                        if(poseList[i] - lastPoseList[i]>=0.1)
                        {poseFlag=1;}
                        lastPoseList[i] = poseList[i];
                    }
                    
                    if(poseFlag==0)
                    {count++;}
                    else
                    {count = 0;}
                }
                catch(std::exception& e)
                {
                    std::cout<<"读取数据异常！！！"<<std::endl;
                    count = 0;
                }
            }
            if(count>=5)
            {
                for(int i=0;i<6;i++)
                {
                    std::cout<<"poseList"<<i<<":"<<poseList[i]<<std::endl;

                }
                homing(poseList);//回零

                /********************************test motor*********************************/
                std::cout<<"test motor6......"<<std::endl;
                loop_rate_slow.sleep();  
                loop_rate_slow.sleep(); 
                loop_rate_slow.sleep();  
                loop_rate_slow.sleep(); 
                for(int iii=0;iii<3;iii++)//测试6轴是否有响应
                {    
                    uint8_t canidlist4[10]={6},cmd4[10]={30};
                    uint32_t parameterlist4[10]={t6+827392};
                    sendCommand(1,canidlist4,cmd4,parameterlist4);
                    loop_rate_slow.sleep();  
                    loop_rate_slow.sleep();  
                    loop_rate_slow.sleep();  
                    loop_rate_slow.sleep();  
                }
                for(int iii=0;iii<3;iii++)//测试6轴是否有响应
                {    
                    uint8_t canidlist4[10]={6},cmd4[10]={30};
                    uint32_t parameterlist4[10]={t6};
                    sendCommand(1,canidlist4,cmd4,parameterlist4);
                    loop_rate_slow.sleep();  
                }
                std::cout<<"tested motor6!"<<std::endl;
                loop_rate_slow.sleep();
                loop_rate_slow.sleep();
            }
        }

    }



    // for(int i = 0; i<3; i++)//电机回零 2 3
    // {
    //     uint8_t canidlist2[10]={2,3},cmd2[10]={30,30};
    //     uint32_t parameterlist2[10]={t2,t3};
    //     sendCommand(2,canidlist2,cmd2,parameterlist2);
    //     loop_rate_slow.sleep();  
       
    // }
    // for(int iii=0;iii<3;iii++)//电机回零 1 4 5 6 7
    // {    
    //     uint8_t canidlist3[10]={1,4,5,6,7},cmd3[10]={30,30,30,30,30};
    //     uint32_t parameterlist3[10]={t1,t4,t5,t6,t7};
    //     sendCommand(5,canidlist3,cmd3,parameterlist3);
    //     loop_rate_slow.sleep();  
        
    // }



/************************************work start*******************************************/
    std::cout<<"***********loop start**************"<<std::endl;  
    int loop_int =0;

    //用于计算读取速率
    double time_read_sp = ros::Time::now().toSec();
    double time_down_data = ros::Time::now().toSec();
    loop_rate_slow.sleep();
    double last_time_down_data = ros::Time::now().toSec();
    double fram_of_do_data = 0 ;
    


    while(ros::ok())
    {
        //获取缓冲区内的字节数
        ////size_t n = sp.available();
        
        ros::spinOnce();

        //不连接matlab 将下列代码注释
        /*+++++++++++++++++++read socket data from matlab+++++++++++++++++*/
        // char sockCReadBuff[1024]={0};
        // memset(sockCReadBuff,0,sizeof(sockCReadBuff));
       
        // if(0 < read(listener.sCfd,sockCReadBuff,3))//3 byte of the msg head
        // { 
        //     // std::cout<<"recv from matlab:"<<sockCReadBuff<<std::endl;
        //     string readStr = sockCReadBuff;
        //     stringstream readSS(readStr);
        //     int msgSize = 0;
        //     readSS >> msgSize;
        //     // std::cout<<"msgSize:"<<msgSize<<std::endl;

        //     if(0 < msgSize)//there are msgSize byte data for read
        //     {
        //         if(0 < read(listener.sCfd,sockCReadBuff,msgSize))
        //         {
        //             std::cout<<"sockCReadBuff:"<<sockCReadBuff<<std::endl;
        //             string readStr2 = sockCReadBuff;
        //             auto jObj = json::parse(readStr2);
                   
                   
        //             //get data to struct jData
        //             jsonData jData = Json2Struct(jObj);

        //             //send joint cmd to ti5_arm
        //             listener.socketCcallback(jData);
        //         }
        //     }
        //     else//there are no data for read
        //     {
        //         //send ack to matlab
        //         int res = write(listener.sCfd,"000",3);
        //     }
        // }
        /*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

        if (loop_int==1)
        {
            std::cout<<"***********loop ="<<loop_int<<"**************"<<std::endl;    
            loop_int=0;
              

            // std::cout<<"msgSetTime:%.4f"<<ros::Time::now().toSec()<<std::endl;
            // printf("msgSetTime:%.4f",ros::Time::now().toSec());

            // std::string msg = "<08010203040506070341414141414141143FFF0000F54B03006DF90000FBBC0500D60200009854FBFF65000000507500006BFF00003DC300000800000029C3000049FF0000D7FFFFFF24000000EC810B8E";
            
            // std::vector<string> res ;
            // res.push_back(msg);
   
            //get joints' I V and S              
            listener.readCommand(poseList);
            time_down_data = ros::Time::now().toSec();
            fram_of_do_data = (1.0/(time_down_data-last_time_down_data));
            std::cout<<"*****+++++++++++fram_of_read_data:"<<fram_of_do_data<<"+++++++++++***********"<<std::endl;
            last_time_down_data = time_down_data;
        

        }
        loop_int+=1;
       
        loop_rate.sleep();
       
    }
    
    //关闭串口
    sp.close();
 
    return 0;
}

